function scan = simulate_laser_scan(pose, obstacles, max_range, fov, num_beams, noise_std)
% 模拟激光雷达扫描
% 输入: pose-位姿[x;y;theta], obstacles-障碍物列表, max_range-最大距离
%       fov-视场角, num_beams-激光束数, noise_std-噪声标准差
% 输出: scan-扫描距离数组

    x = pose(1);
    y = pose(2);
    theta = pose(3);
    
    % 激光束角度
    angle_min = theta - fov/2;
    angle_max = theta + fov/2;
    angles = linspace(angle_min, angle_max, num_beams);
    
    scan = zeros(num_beams, 1);
    
    for i = 1:num_beams
        angle = angles(i);
        
        % 光线方向
        dx = cos(angle);
        dy = sin(angle);
        
        % 光线投射，找最近障碍物
        min_dist = max_range;
        
        for j = 1:size(obstacles, 1)
            obs = obstacles(j, :);
            dist = ray_box_intersection(x, y, dx, dy, obs);
            if dist > 0 && dist < min_dist
                min_dist = dist;
            end
        end
        
        % 添加噪声
        scan(i) = min_dist + randn() * noise_std;
        scan(i) = max(0.1, min(scan(i), max_range));  % 限制范围
    end
end

function dist = ray_box_intersection(x0, y0, dx, dy, box)
% 光线与矩形框相交检测
% box: [x_min, y_min, x_max, y_max]

    x_min = box(1);
    y_min = box(2);
    x_max = box(3);
    y_max = box(4);
    
    % 光线参数方程: (x0 + t*dx, y0 + t*dy)
    % 计算与四条边的交点
    
    t_min = -inf;
    t_max = inf;
    
    % X方向
    if abs(dx) > 1e-6
        t1 = (x_min - x0) / dx;
        t2 = (x_max - x0) / dx;
        t_min = max(t_min, min(t1, t2));
        t_max = min(t_max, max(t1, t2));
    else
        if x0 < x_min || x0 > x_max
            dist = -1;
            return;
        end
    end
    
    % Y方向
    if abs(dy) > 1e-6
        t1 = (y_min - y0) / dy;
        t2 = (y_max - y0) / dy;
        t_min = max(t_min, min(t1, t2));
        t_max = min(t_max, max(t1, t2));
    else
        if y0 < y_min || y0 > y_max
            dist = -1;
            return;
        end
    end
    
    % 检查是否有交点
    if t_min > t_max || t_max < 0
        dist = -1;
    elseif t_min > 0
        dist = t_min;
    else
        dist = -1;  % 起点在障碍物内
    end
end

